/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.HardwareMap;
import edu.wpi.first.wpilibj.CounterBase;

public class DriveTrain extends Subsystem {
    
    //Sets how far the wheels move per one pulse of the encoder
    public static final double ENCODER_DISTANCE_PER_PULSE = ((8.0/12)*3.14)/(250*3); //.0027925268
    
    RobotDrive Drive; 
    
    //Assigns jaguars 
    CANJaguar FrontLeft;
    CANJaguar RearLeft;
    CANJaguar FrontRight;
    CANJaguar RearRight;
    
    //Declares encoder PWM ports, if they are reversed, and the type (2x in this case) 
    Encoder LeftEncoder = new Encoder(HardwareMap.LEFT_DRIVE_ENCODER_CH_A,
            HardwareMap.LEFT_DRIVE_ENCODER_CH_B, true, CounterBase.EncodingType.k2X);

    //Right encoder is reveresed because the encoders are in opposite directions of each other
    Encoder RightEncoder = new Encoder(HardwareMap.RIGHT_DRIVE_ENCODER_CH_A,
            HardwareMap.RIGHT_DRIVE_ENCODER_CH_B, false, CounterBase.EncodingType.k2X);

    public DriveTrain() {
       super("DriveTrain");
         //Outputs "Hello" to the computer's output screen
         System.out.println("Hello");
       
       //Sets up the jaguar positions
       try {
           FrontLeft = new CANJaguar(HardwareMap.FRONT_LEFT_JAGUAR);
           RearLeft = new CANJaguar(HardwareMap.REAR_LEFT_JAGUAR);
           FrontRight = new CANJaguar(HardwareMap.FRONT_RIGHT_JAGUAR);
           RearRight = new CANJaguar(HardwareMap.REAR_RIGHT_JAGUAR);  
        }
       catch (CANTimeoutException e){
           e.printStackTrace();
        }
            
        Drive = new RobotDrive(FrontLeft,RearLeft,FrontRight,RearRight);
                
                
        //Sets distance in feet
        //This was obtained by finding the circumfrance of the wheels,
        //finding the number of encoder pulses per rotation,
        //and dividing the circumfrance by that number.
        
        LeftEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
        RightEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
    }
  
    public void initDefaultCommand() {
    }
    
    //Creates functions to be used elsewhere in code
    public void TankDrive(double LeftValue,double RightValue){
        Drive.tankDrive(LeftValue, RightValue);
    }
    
    public void ArcadeDrive(double MoveValue, double RotateValue){
        Drive.arcadeDrive(RotateValue, MoveValue);
    }
        
    public double GetLeftEncoderValue(){
        return(LeftEncoder.getDistance());
    }
    
    public double GetRightEncoderValue(){
       return(RightEncoder.getDistance());
    }
    
    public void StartEncoders(){
        LeftEncoder.start();
        RightEncoder.start();
    }

    public void StopEncoders(){
        LeftEncoder.stop();
        RightEncoder.stop();
    }

    public void ResetEncoders(){
        LeftEncoder.reset();
        RightEncoder.reset();
    }
}